#include "serial_to_stm32/incoming_msg.h"
#include "serial_to_stm32/outgoing_msg.h"
#include "serial_to_stm32/shoot_status_t.h"
#include <cstdint>
#include <iostream>
#include <ros/ros.h>
#include <stdio.h>
#include <stdlib.h>
#include <string>

#define TTY_PATH "/dev/tty"
#define STTY_US "stty raw -echo -F "
#define STTY_DEF "stty -raw echo -F "

#define PI 3.1415926536

int get_char(void);
void GimbalFeedbackCallback(const serial_to_stm32::incoming_msg::ConstPtr &msg);
float loop_limit(float input);

serial_to_stm32::incoming_msg gimbal_feedback_data;

int last_auto_status;

int main(int argc, char **argv) {
	ros::init(argc, argv, "key_input_debug");

	ros::NodeHandle n;
	ros::Publisher gimbal_control_pub =
	    n.advertise<serial_to_stm32::outgoing_msg>("gimbal_control", 1);
	ros::Subscriber gimbal_feedback_sub =
	    n.subscribe("gimbal_feedback", 1, GimbalFeedbackCallback);
	serial_to_stm32::outgoing_msg gimbal_control_data;
	system(STTY_US TTY_PATH);
	gimbal_control_data.gimbal_yaw = gimbal_feedback_data.gimbal_yaw;
	gimbal_control_data.gimbal_pitch = gimbal_feedback_data.gimbal_pitch;
	gimbal_control_data.shoot_status = gimbal_feedback_data.shoot_status;
	ros::Rate loop_rate(1000);
	int count = 0;
	while (ros::ok()) {
		ros::spinOnce();
		if (last_auto_status == 0 && gimbal_feedback_data.auto_status == 1) {
			ROS_INFO("swith into auto mode");
			gimbal_control_data.gimbal_pitch =
			    gimbal_feedback_data.gimbal_pitch;
			gimbal_control_data.gimbal_yaw = gimbal_feedback_data.gimbal_yaw;
		} else if (last_auto_status == 1 &&
		           gimbal_feedback_data.auto_status == 0) {
			ROS_INFO("swith out auto mode");
		}
		int ch = get_char();
		
		    if (ch != 0) {
		        printf("%d\n\r", ch);
		    }
		
		if (ch == 3) { // ctrl+c
			system(STTY_DEF TTY_PATH);
			return 0;
		}

		switch (ch) {
		case 119: // w
			gimbal_control_data.gimbal_pitch -= 0.00847266;
			break;
		case 115: // s
			gimbal_control_data.gimbal_pitch += 0.00847266;
			break;
		case 97: // a
			gimbal_control_data.gimbal_yaw += 0.03627266;
			break;
		case 100: // d
			gimbal_control_data.gimbal_yaw -= 0.030627266;
			break;
		case 48: // 0
			gimbal_control_data.shoot_status.state = 0;
			break;
		case 96: // `
			gimbal_control_data.shoot_status.bullets_count = 0;
			break;
		case 49: // 1
			gimbal_control_data.shoot_status.bullets_count = 1;
			break;
		case 50: // 2
			gimbal_control_data.shoot_status.bullets_count = 2;
			break;
		case 51: // 3
			gimbal_control_data.shoot_status.bullets_count = 3;
			break;
		case 52: // 4
			gimbal_control_data.shoot_status.bullets_count = 4;
			break;
		case 53: // 5
			gimbal_control_data.shoot_status.bullets_count = 5;
			break;
		case 54: // 6
			gimbal_control_data.shoot_status.bullets_count = 6;
			break;
		case 55: // 7
			gimbal_control_data.shoot_status.bullets_count = 7;
			break;
		case 45: // -
			gimbal_control_data.shoot_status.state = 1;
			break;
		case 61: // =
			gimbal_control_data.shoot_status.state = 2;
			break;
		case 104: // h
			gimbal_control_data.shoot_status.frequency += 0.2;
			break;
		case 108: // l
			gimbal_control_data.shoot_status.frequency -= 0.2;
			break;
		default: {
			// gimbal_control_data.gimbal_pitch = 0;
			// gimbal_control_data.gimbal_yaw = 0;
		} break;
		}

		gimbal_control_data.gimbal_yaw =
		    loop_limit(gimbal_control_data.gimbal_yaw);

		gimbal_control_data.gimbal_pitch =
		    (gimbal_control_data.gimbal_pitch >= 0.49)
		        ? (0.49)
		        : ((gimbal_control_data.gimbal_pitch <= -0.67)
		               ? -0.67
		               : gimbal_control_data.gimbal_pitch);

		gimbal_control_data.shoot_status.frequency =
		    (gimbal_control_data.shoot_status.frequency >= 15)
		        ? (15)
		        : ((gimbal_control_data.shoot_status.frequency <= 0.2)
		               ? 0.2
		               : gimbal_control_data.shoot_status.frequency);

		// publish message
		gimbal_control_pub.publish(gimbal_control_data);
		/*ROS_INFO("Publish Gimbal Control:\nyaw: %f,  pitch :%f,  shoot_status:
		   "
		         "%d|%d|%d\r\n",
		         gimbal_control_data.gimbal_yaw,
		         gimbal_control_data.gimbal_pitch,
		         gimbal_control_data.shoot_status.state,
		         gimbal_control_data.shoot_status.bullets_count,
		         gimbal_control_data.shoot_status.frequency);*/

		// set delay time according to loop frequency
		loop_rate.sleep();
	}

	return 0;
}

void GimbalFeedbackCallback(
    const serial_to_stm32::incoming_msg::ConstPtr &msg) {
	last_auto_status = gimbal_feedback_data.auto_status;

	gimbal_feedback_data.gimbal_yaw = msg->gimbal_yaw;
	gimbal_feedback_data.gimbal_pitch = msg->gimbal_pitch;
	gimbal_feedback_data.shoot_status.state = msg->shoot_status.state;
  gimbal_feedback_data.shoot_status.bullets_count = msg->shoot_status.bullets_count;
  gimbal_feedback_data.shoot_status.frequency = msg->shoot_status.frequency;
  gimbal_feedback_data.auto_status = msg->auto_status;
	ROS_INFO("Receive Gimbal Feedback:\nyaw: %f,  pitch :%f,  shoot_status:"
	         "%d|%d|%f,auto_status: %d\r\n",
	         gimbal_feedback_data.gimbal_yaw, gimbal_feedback_data.gimbal_pitch,
	         gimbal_feedback_data.shoot_status.state,
	         gimbal_feedback_data.shoot_status.bullets_count,
	         gimbal_feedback_data.shoot_status.frequency,
	         gimbal_feedback_data.auto_status);
}

int get_char(void) {
	fd_set rfds;
	struct timeval tv;
	int ch = 0;

	FD_ZERO(&rfds);
	FD_SET(0, &rfds);
	tv.tv_sec = 0;
	tv.tv_usec = 2; //���õȴ���ʱʱ��

	//�������Ƿ�������
	if (select(1, &rfds, NULL, NULL, &tv) > 0) {
		ch = getchar();
	}
	return ch;
}

float loop_limit(float input) {
	while (input <= -PI) {
		input += 2 * PI;
	}

	while (input >= PI) {
		input -= 2 * PI;
	}

	return input;
}
